package sdp;

import java.awt.event.KeyEvent;
import java.awt.event.KeyListener;

import javax.swing.JButton;
import javax.swing.JSlider;

import sdp.Camera.CommunicationError;
import sdp.Camera.ConnectionFailed;
import sdp.ControlPanel.SliderCallback;

import java.io.IOException;

import lejos.pc.comm.NXTComm;
import lejos.pc.comm.NXTCommException;
import lejos.pc.comm.NXTCommFactory;
import lejos.pc.comm.NXTInfo;

/** NxtWorld extend World, to provide access to the real, physical robot, this description is copied from World.java, which explains the design in more detail:
 *   The NxtWorld corresponds to the real, physical world. The desired wheel speeds are communicated to the robot, using the
 *   Communication class. The latter is also used to fetch an image from the camera which is processed by a Vision object to
 *   estimate the new positions and orientations which will be available to the Strategy in the next time step.
 */
public class NxtWorld extends World {
	private Camera        cam;
	private Communication comm;
	private Vision        cv;
	private ImageViewer   viewer;
	private ControlPanel  panel;
	private boolean       addedCameraSliders = false;
	
	public NxtWorld( World.Settings settings, ImageViewer viewer, ControlPanel panel ) {
		super( settings );
		
		// TODO Auto-generated method stub
		this.comm   = new Communication();
		this.cam    = new Camera();
		this.cv     = new Vision();
		this.panel  = panel;
		this.viewer = viewer;
	}
	
	@Override
	protected void update() {
		// if the communication was lost, or this is the first update, attempt connection to the robot.
		if( cam.isConnected() && comm.isConnected() ) {
			// communicate with the robot, passing the motor speeds and getting the sensor values.
			comm.update( motorSpeeds );
			encoders = comm.getEncoderValues();
			bumper   = cam.isBumperPressed();
			try {
				viewer.showImage( cv.processImage( cam.getCameraImage(), getPlayerState(), getOpponentState(), getBallState() ) );
				// viewer.showImage( cv.drawChessboardCorners( comm.getCameraImage() ) );
			} catch( Exception e ) {
				e.printStackTrace();
				System.err.println("NxtWorld: Should not happen.");
			}
			// processImage() updates the states by reference.
			
		} else {
			System.out.println("NxtWorld: Not connected. Connecting...");
			if( !cam.isConnected() ) {	
				try {
					cam.connect();
					if( !addedCameraSliders ) {
						addedCameraSliders = true;
						addCameraSliders();
						panel.addKeyListener( new KeyListener() {
							
							@Override
							public void keyTyped(KeyEvent e) {
								boolean cropChanged = false;
								if( e.getKeyChar() == 'w' ) {
									cv.setCropTop( cv.getCropTop() - 1 ); cropChanged = true;
								} else if( e.getKeyChar() == 's' ) {
									cv.setCropTop( cv.getCropTop() + 1 ); cropChanged = true;
								} else if( e.getKeyChar() == 'a' ) {
									cv.setCropLeft( cv.getCropLeft() - 1 ); cropChanged = true;
								} else if( e.getKeyChar() == 'd' ) {
									cv.setCropLeft( cv.getCropLeft() + 1 ); cropChanged = true;
								} else if( e.getKeyChar() == 'h' ) {
									cv.setCropWidth( cv.getCropWidth() + 1 ); cropChanged = true;
								} else if( e.getKeyChar() == 'f' ) {
									cv.setCropWidth( cv.getCropWidth() - 1 ); cropChanged = true;
								} else if( e.getKeyChar() == 't' ) {
									cv.setCropHeight( cv.getCropHeight() - 1 ); cropChanged = true;
								} else if( e.getKeyChar() == 'g' ) {
									cv.setCropHeight( cv.getCropHeight() + 1 ); cropChanged = true;
								}
								
								if( cropChanged ) {
									System.out.print("NxtWorld: New Crop Rectangle: ( ");
									System.out.print( cv.getCropLeft() + ", " + cv.getCropTop() + " ; " );
									System.out.println( cv.getCropWidth() + ", " + cv.getCropHeight() );
								}
							}
							
							@Override
							public void keyReleased(KeyEvent arg0) {
								// TODO Auto-generated method stub
								
							}
							
							@Override
							public void keyPressed(KeyEvent e) {
								if( e.getKeyCode() == KeyEvent.VK_F1 ) {
									cv.setMode( Vision.Mode.RAW );
								} else if( e.getKeyCode() == KeyEvent.VK_F2 ) {
									cv.setMode( Vision.Mode.CHANNELS );
								} else if( e.getKeyCode() == KeyEvent.VK_F3 ) {
									//cv.setMode( Vision.Mode.PITCH );
								}
							}
						});
					}
				} catch( ConnectionFailed e ) {
					e.printStackTrace();
					System.err.println("NxtWorld: Failed to connect, trying again next time step.");
				} catch( CommunicationError e ) {
					e.printStackTrace();
					System.err.println("NxtWorld: Connection error.");
				}
			}
			
			if( !comm.isConnected() ) {
				try {
					comm.connect();
				} catch( IOException e ) {
					e.printStackTrace();
					System.err.println("NxtWorld: Connection error.");
				}
			}
		}
	}
	
	private void addCameraSliders() throws CommunicationError {
		panel.addSlider("Brightness", cam.getBrightness(), 0, 255, new SliderCallback() {
			@Override
			public void valueChanged(JSlider s, int newValue) {
				cam.setBrightness(newValue);
				System.out.println("brightness="+newValue);
			}
		});
		
		panel.addSlider("Contrast", cam.getContrast(), 0, 127, new SliderCallback() {
			@Override
			public void valueChanged(JSlider s, int newValue) {
				cam.setContrast(newValue);
				System.out.println("contrast="+newValue);
			}
		});
		
		panel.addSlider("Saturation", cam.getSaturation(), 0, 127, new SliderCallback() {
			@Override
			public void valueChanged(JSlider s, int newValue) {
				cam.setSaturation(newValue);
				System.out.println("saturation="+newValue);
			}
		});
		
		
		panel.addSlider("Hue", cam.getHue(), -128, 127, new SliderCallback() {
			@Override
			public void valueChanged(JSlider s, int newValue) {
				cam.setHue( newValue );
				System.out.println("hue="+newValue);
			}
		});
		
		panel.addSlider("AGC", cam.getChromaAGC(), 0, 1, new SliderCallback() {
			@Override
			public void valueChanged(JSlider s, int newValue) {
				cam.setChromaAGC( newValue );
				System.out.println("agc="+newValue);
			}
		});
		
		panel.addSlider("Gain", cam.getGain(), 0, 127, new SliderCallback() {
			@Override
			public void valueChanged(JSlider s, int newValue) {
				cam.setGain( newValue );
				System.out.println("gain="+newValue);
			}
		});
		
		panel.addSlider("Min. Hue", cv.getMinHue(adjustedChannel), 0, 255, new SliderCallback() {
			@Override
			public void valueChanged(JSlider s, int newValue) {
				cv.setMinHue( adjustedChannel, newValue );
				System.out.println("minHue="+newValue);
			}
		});
		
		panel.addSlider("Max. Hue", cv.getMaxHue(adjustedChannel), 0, 255, new SliderCallback() {
			@Override
			public void valueChanged(JSlider s, int newValue) {
				cv.setMaxHue( adjustedChannel, newValue );
				System.out.println("maxHue="+newValue);
			}
		});
		
		panel.addSlider("Min. Value", cv.getMinValue(), 0, 255, new SliderCallback() {
			@Override
			public void valueChanged(JSlider s, int newValue) {
				cv.setMinValue( newValue );
				System.out.println("minValue="+newValue);
			}
		});
		
		panel.addSlider("Min. Saturation", cv.getMinSaturation(), 0, 255, new SliderCallback() {
			@Override
			public void valueChanged(JSlider s, int newValue) {
				cv.setMinSaturation( newValue );
				System.out.println("minSaturation="+newValue);
			}
		});
		
		panel.addSlider("Channel (Red, Yellow, Blue, Pitch)", 0, 0, 3, new SliderCallback() {
			@Override
			public void valueChanged( JSlider s, int newValue) {
				adjustedChannel = newValue;
				panel.getSlider("Max. Hue").setValue( cv.getMaxHue(adjustedChannel) );
				panel.getSlider("Min. Hue").setValue( cv.getMinHue(adjustedChannel) );
			}
		});
		
		panel.addButton("Auto Crop", new ControlPanel.ButtonCallback() {
			@Override
			public void buttonClicked(JButton b) {
				//viewer.showImage(cv.autoCrop());
			}
		});
	}
	
	static private int adjustedChannel = 0;
}
